/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */

#include "main.h"
#include "interp.h"
#include <stdio.h>

static void InterpolateSubFrame(
float	cur_lsf[ORDER],
float	prev_lsf[ORDER],
float	lsfint[NUM_SF][ORDER],
int	sf);

static void AnalCheckForMonotonicity(
float	cur_lsfint[NUM_SF][ORDER],
float	prev_lsfint[ORDER],
int	sf,
int	frame_num);

static void SynthCheckForMonotonicity(
float	cur_lsf[ORDER],
float	prev_lsf[ORDER]);

/**************************************************************************
*                                                                         *
* ROUTINE
*		Interpolate
*
* FUNCTION
*		Interpolation of LSFs
* SYNOPSIS
*		Interpolate(lsf, lsfint, where, frame_num)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	lsf		float	 i	Frame of LSFs
*	lsfint		float	 o	Frame of interpolated LSFs
*	where		int	 i	analysis/synthesis flag
*	frame_num	int	 i	Current frame number
*
**************************************************************************
*
* CALLED BY
*
*	Analysis Synthesis
*
**************************************************************************/
void Interpolate(
float 	lsf[ORDER],
float	lsfint[NUM_SF][ORDER],
int 	where,
int	frame_num)
{
int		i;
static float	prev_ana_sf_lsf[ORDER];
static float	prev_ana_ff_lsf[ORDER]= { 	0.03, 0.05, 0.09, 0.13, 0.19, 
						0.23, 0.29, 0.33, 0.39, 0.44
				   	};
static float	prev_syn_lsf[ORDER]= 	{ 	0.03, 0.05, 0.09, 0.13, 0.19, 
						0.23, 0.29, 0.33, 0.39, 0.44
				   	};

/*  Perform interpolation depending on whether this is analysis or synthesis */
switch(where)	{
/*  Analysis interpolation */
/*-----------------------------------------------------------------------*/
	case ANALYSIS: 
	  for(i=0;i<NUM_SF;i++)	{

/*  Interpolate for each subframe into interpolation matrix */
	    InterpolateSubFrame(lsf, prev_ana_ff_lsf, lsfint, i);

/*  Check for Nonmonotonic LSFs */
	    AnalCheckForMonotonicity(lsfint, prev_ana_sf_lsf, i, frame_num);

	  }

/*  Update values for next pass */
	  for(i=0;i<ORDER;i++)	{
	    /*  Update previous subframe LSFs */
	    prev_ana_sf_lsf[i] = lsfint[NUM_SF-1][i];
	    /*  Update previous full frame LSFs */
	    prev_ana_ff_lsf[i] = lsf[i];
	  }

	  break;
/*-----------------------------------------------------------------------*/
	case SYNTHESIS:

/*  Check for Nonmonotonic LSFs */
	  SynthCheckForMonotonicity(lsf, prev_syn_lsf);

/*  Interpolate LSFs for each subframe into interpolation matrix */
	  for(i=0;i<NUM_SF;i++)	{
	    InterpolateSubFrame(lsf, prev_syn_lsf, lsfint, i);
	  }

/*  Update LSF history */
	  for(i=0;i<ORDER;i++)	{
	    prev_syn_lsf[i] = lsf[i];
	  }

	  break;
/*-----------------------------------------------------------------------*/
}
}

/**************************************************************************
*                                                                         *
* ROUTINE
*		InterpolateSubFrame
*
* FUNCTION
*		Interpolation one LSF subframe
* SYNOPSIS
*		InterpolateSubFrame(cur_lsf, prev_lsf, lsfint, sf)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	cur_lsf		float	 i	Current Frame of LSFs
*	prev_lsf	float	 i	Previous Frame of LSFs
*	lsfint		float	 o	Frame of interpolated LSFs
*	sf		int	 i	Current subframe number
*
**************************************************************************/

float win[2][4] = {	0.875, 0.625, 0.375, 0.125,  
                        0.125, 0.375, 0.625, 0.875	};


void InterpolateSubFrame(
float	cur_lsf[ORDER],
float	prev_lsf[ORDER],
float	lsfint[NUM_SF][ORDER],
int	sf)
{
int		i;

/*  Interpolate for this subframe */
	for(i=0;i<ORDER;i++)	{
	  lsfint[sf][i] = win[0][sf] * prev_lsf[i]  +  win[1][sf] * cur_lsf[i];
	}

}

/**************************************************************************
*                                                                         *
* ROUTINE
*		AnalCheckForMonotonicity
*
* FUNCTION
*		Check for nonmonotonic LSFs within interpolated matrix
* SYNOPSIS
*		AnalCheckForMonotonicity(cur_lsfint, prev_lsfint, sf, frame_num)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	cur_lsfint	float	i/o	Current frame of interpolated LSFs
*	prev_lsfint	float	 o	Previous frame of interpolated LSFs
*	sf		int	 i	Current sub frame number
*	frame_num	int	 i	Current frame number
*
**************************************************************************/

void AnalCheckForMonotonicity(
float	cur_lsfint[NUM_SF][ORDER],
float	prev_lsfint[ORDER],
int	sf,
int	frame_num)
{
int	nonmono=FALSE;
int	i;
float	temp;

/*  Check for nonotonically increasing LSFs, swap crossed LSFs */
	for(i=1;i<ORDER;i++)	{
	  if(cur_lsfint[sf][i] <= cur_lsfint[sf][i-1])	{
	    printf("Analysis Interpolation:\n\tSwapping nonmono LSFs @ frame %d\n", frame_num);
	    temp = cur_lsfint[sf][i];
	    cur_lsfint[sf][i] = cur_lsfint[sf][i-1];
	    cur_lsfint[sf][i-1] = temp;
	  }
	}

/*  Recheck for monotonically increasing LSFs and substitute old LSFs */
	for(i=1;i<ORDER;i++)	{
	  if (cur_lsfint[sf][i] <= cur_lsfint[sf][i-1])
	    nonmono = TRUE;
	}
	if (nonmono)	{
	  printf("Analysis Interpolation:\n\tResetting interpolated LSF at frame %d\n",frame_num);
	  for (i=0;i<ORDER;i++)	{
	    if(i==0)
	      cur_lsfint[sf][i] = prev_lsfint[i];
	    else
	      cur_lsfint[sf][i] = cur_lsfint[sf-1][i];
	  }
	}

}
/**************************************************************************
*                                                                         *
* ROUTINE
*		SynthCheckForMonotonicity
*
* FUNCTION
*		Check for nonmonotonic LSFs 
* SYNOPSIS
*		SynthCheckForMonotonicity()
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*	cur_lsf		float	i/o	Current frame's LSFs
*	prev_lsf	float	 i	Previous frame's LSFs
*
**************************************************************************/
void SynthCheckForMonotonicity(
float	cur_lsf[ORDER],
float	prev_lsf[ORDER])
{
int	i;
int	nonmono;

/*  Try to fix any nonmonotonic LSFs by repeating pair	*/
	for (i = 1; i < ORDER; i++)
	{
	  if (cur_lsf[i] <= cur_lsf[i - 1])
	  {
	    printf("Synthesis Interpolation: try to fix any nonmonotonic LSFs\n");
	    cur_lsf[i] = prev_lsf[i];
	    cur_lsf[i - 1] = prev_lsf[i - 1];
    	  }
  	}
/*  Check fixed LSPs (also check for pairs too close?)	*/

  	nonmono = FALSE;
  	for (i = 1; i < ORDER; i++)
  	{
  	  if (cur_lsf[i] <= cur_lsf[i - 1])
    	  nonmono = TRUE;
  	}

/*  If fix fails, repeat entire LSP vector		*/

  	if (nonmono)
  	{
  	  printf("Synthesis Interpolation: repeat entire LSP vector\n");
  	  for (i = 0; i < ORDER; i++)
  	  {
  	    cur_lsf[i] = prev_lsf[i];
  	  }
  	}


}
